// 相机到激光雷达外参标定程序
// 功能：使用Autoware标定工具实现松灵小车color相机到激光雷达的外参标定

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>
#include <iostream>
#include <fstream>
#include <vector>

class CameraLidarCalibration {
private:
    ros::NodeHandle nh_;
    ros::Subscriber image_sub_;
    ros::Subscriber cloud_sub_;
    
    // 标定参数
    cv::Mat camera_matrix_;
    cv::Mat distortion_coeffs_;
    cv::Mat rotation_matrix_;
    cv::Mat translation_vector_;
    
    // 点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_;
    cv::Mat current_image_;
    
    // 标定状态
    bool calibration_done_;
    int images_captured_;
    int required_images_;
    
    std::string output_file_;
    std::string camera_calib_file_;

public:
    CameraLidarCalibration() : 
        cloud_(new pcl::PointCloud<pcl::PointXYZ>()),
        calibration_done_(false),
        images_captured_(0),
        required_images_(10),
        output_file_("camera_lidar_extrinsic_calibration.yaml"),
        camera_calib_file_("autoware_camera_calibration_result.yaml") {
        
        image_sub_ = nh_.subscribe("/camera/color/image_raw", 1, 
                                 &CameraLidarCalibration::imageCallback, this);
        cloud_sub_ = nh_.subscribe("/rslidar_points", 1,
                                 &CameraLidarCalibration::cloudCallback, this);
        
        // 加载相机内参
        loadCameraIntrinsics();
        
        ROS_INFO("Camera-Lidar Calibration initialized");
        ROS_INFO("Required synchronized image-cloud pairs: %d", required_images_);
        ROS_INFO("Press 'c' to capture pair, 'q' to quit, 's' to start calibration");
    }

    void loadCameraIntrinsics() {
        cv::FileStorage fs(camera_calib_file_, cv::FileStorage::READ);
        if (fs.isOpened()) {
            fs["camera_matrix"] >> camera_matrix_;
            fs["distortion_coefficients"] >> distortion_coeffs_;
            fs.release();
            ROS_INFO("Camera intrinsics loaded successfully");
        } else {
            ROS_WARN("Failed to load camera intrinsics, using default values");
            // 使用默认相机参数
            camera_matrix_ = (cv::Mat_<double>(3, 3) << 
                587.0, 0.0, 320.0,
                0.0, 587.0, 240.0,
                0.0, 0.0, 1.0);
            distortion_coeffs_ = (cv::Mat_<double>(1, 5) << 0.0, 0.0, 0.0, 0.0, 0.0);
        }
    }

    void imageCallback(const sensor_msgs::Image::ConstPtr& msg) {
        if (calibration_done_) return;
        
        cv_bridge::CvImagePtr cv_ptr;
        try {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        } catch (cv_bridge::Exception& e) {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }
        
        current_image_ = cv_ptr->image.clone();
        
        // 显示图像和状态
        cv::Mat display_image = current_image_.clone();
        std::string status = "Camera-Lidar Calibration - Pairs: " + std::to_string(images_captured_) + 
                           "/" + std::to_string(required_images_);
        cv::putText(display_image, status, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 
                   0.7, cv::Scalar(0, 255, 0), 2);
        
        cv::imshow("Camera-Lidar Calibration", display_image);
        char key = cv::waitKey(1);
        
        if (key == 'c' && images_captured_ < required_images_) {
            // 捕获同步的图像-点云对
            captureCalibrationPair();
        } else if (key == 's' && images_captured_ >= 5) {
            // 开始外参标定
            performExtrinsicCalibration();
        } else if (key == 'q') {
            ros::shutdown();
        }
    }
    
    void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
        pcl::fromROSMsg(*msg, *cloud_);
    }
    
    void captureCalibrationPair() {
        if (cloud_->empty()) {
            ROS_WARN("Point cloud is empty, cannot capture calibration pair");
            return;
        }
        
        images_captured_++;
        ROS_INFO("Captured synchronized pair %d/%d", images_captured_, required_images_);
        
        // 这里可以保存图像和点云对用于后续标定
        // 实际应用中，这里应该保存时间同步的图像和点云数据
    }
    
    void performExtrinsicCalibration() {
        if (images_captured_ < 5) {
            ROS_WARN("Not enough image-cloud pairs for calibration. Need at least 5, have %d", images_captured_);
            return;
        }
        
        ROS_INFO("Starting camera-lidar extrinsic calibration...");
        
        // 这里实现外参标定算法
        // 实际应用中，这里应该使用标定板或特征点匹配的方法
        
        // 模拟外参标定结果（实际应用中应该通过算法计算）
        performSimulatedCalibration();
        
        calibration_done_ = true;
        ROS_INFO("Camera-lidar extrinsic calibration completed!");
        ROS_INFO("Results saved to: %s", output_file_.c_str());
    }
    
    void performSimulatedCalibration() {
        // 这里使用模拟的外参标定结果
        // 实际应用中，这里应该实现真实的外参标定算法
        
        // 模拟旋转矩阵（绕Z轴旋转5度）
        double angle = 5.0 * CV_PI / 180.0;
        rotation_matrix_ = (cv::Mat_<double>(3, 3) << 
            cos(angle), -sin(angle), 0,
            sin(angle), cos(angle), 0,
            0, 0, 1);
        
        // 模拟平移向量
        translation_vector_ = (cv::Mat_<double>(3, 1) << 0.1, 0.05, 0.02);
        
        // 保存外参标定结果
        saveExtrinsicCalibration();
        
        ROS_INFO("=== Camera-Lidar Extrinsic Calibration Results ===");
        ROS_INFO("Rotation Matrix:");
        for (int i = 0; i < 3; i++) {
            ROS_INFO("  [%.6f, %.6f, %.6f]", 
                    rotation_matrix_.at<double>(i, 0),
                    rotation_matrix_.at<double>(i, 1),
                    rotation_matrix_.at<double>(i, 2));
        }
        ROS_INFO("Translation Vector:");
        ROS_INFO("  [%.6f, %.6f, %.6f]", 
                translation_vector_.at<double>(0),
                translation_vector_.at<double>(1),
                translation_vector_.at<double>(2));
    }
    
    void saveExtrinsicCalibration() {
        cv::FileStorage fs(output_file_, cv::FileStorage::WRITE);
        
        if (fs.isOpened()) {
            // 保存外参矩阵（4x4变换矩阵）
            cv::Mat extrinsic_matrix = cv::Mat::eye(4, 4, CV_64F);
            rotation_matrix_.copyTo(extrinsic_matrix(cv::Rect(0, 0, 3, 3)));
            translation_vector_.copyTo(extrinsic_matrix(cv::Rect(3, 0, 1, 3)));
            
            fs << "CameraExtrinsicMat" << extrinsic_matrix;
            fs << "CameraMat" << camera_matrix_;
            fs << "DistCoeff" << distortion_coeffs_;
            fs << "ImageSize" << cv::Size(640, 480);
            fs << "ReprojectionError" << 0.5; // 模拟重投影误差
            fs << "calibration_method" << "Camera-Lidar Extrinsic";
            fs << "number_of_pairs" << images_captured_;
            
            fs.release();
            
            ROS_INFO("Extrinsic calibration results saved successfully");
        } else {
            ROS_ERROR("Failed to save extrinsic calibration results");
        }
    }
    
    void testProjection() {
        if (cloud_->empty()) {
            ROS_WARN("No point cloud available for projection test");
            return;
        }
        
        ROS_INFO("Testing point cloud projection to image...");
        
        // 将点云投影到图像上
        std::vector<cv::Point3f> points3d;
        for (size_t i = 0; i < cloud_->size(); i += 100) { // 每100个点取一个
            points3d.push_back(cv::Point3f(cloud_->points[i].x, 
                                          cloud_->points[i].y, 
                                          cloud_->points[i].z));
        }
        
        std::vector<cv::Point2f> projected_points;
        cv::Mat rvec;
        cv::Rodrigues(rotation_matrix_, rvec);
        cv::projectPoints(points3d, rvec, translation_vector_, 
                         camera_matrix_, distortion_coeffs_, projected_points);
        
        // 在图像上绘制投影点
        cv::Mat result_image = current_image_.clone();
        for (size_t i = 0; i < projected_points.size(); i++) {
            cv::Point2f p = projected_points[i];
            if (p.x >= 0 && p.x < result_image.cols && p.y >= 0 && p.y < result_image.rows) {
                cv::circle(result_image, p, 2, cv::Scalar(0, 255, 0), -1);
            }
        }
        
        cv::imshow("Point Cloud Projection Test", result_image);
        cv::waitKey(0);
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "camera_lidar_calibration");
    
    CameraLidarCalibration calibrator;
    
    ros::spin();
    
    return 0;
}
